﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Net;
using System.IO;
using System.IO.Compression;
using System.Net.Sockets;
using System.Text;
using System.Windows;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;

namespace KinectServerWPF
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        private KinectSensor kinect;
        private Socket depthSocket;

        public MainWindow()
        {
            //KinectServerCommon.BitCompressor b = new KinectServerCommon.BitCompressor();

            InitializeComponent();

            fpsQueue = new Queue<DateTime>();
            Task.Factory.StartNew(new Action(() => Init()));
        }

        private void UpdateFps()
        {
            if (fpsQueue.Count < 10)
                fpsQueue.Enqueue(DateTime.Now);
            else
            {
                fpsQueue.Enqueue(DateTime.Now);
                TimeSpan diff = DateTime.Now - fpsQueue.Dequeue();
                double fps = 10 / diff.TotalSeconds;

                Dispatcher.BeginInvoke(new Action(() => this.txtFps.Text = string.Format("{0:F1}", fps)));
            }
        }

        private Queue<DateTime> fpsQueue;

        private void Init()
        {
            InitNetwork();

            InitKinect();
        }

        private void InitSocket()
        {
            string IPString = PrefLoader.GetString("TargetIP");

            depthSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
            depthSocket.Connect(IPAddress.Parse(IPString), 20161);
        }

        private void InitKinect()
        {
            while (KinectSensor.KinectSensors.Count == 0) ;

            kinect = KinectSensor.KinectSensors[0];

            kinect.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
            kinect.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
            kinect.Start();

            kinect.AllFramesReady += kinect_AllFramesReady;

            //kinect = Runtime.Kinects[0];
            //options = RuntimeOptions.UseDepth;
            //kinect.Initialize(options);

            //kinect.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240,
            //options.HasFlag(RuntimeOptions.UseDepthAndPlayerIndex) || options.HasFlag(RuntimeOptions.UseSkeletalTracking) ? ImageType.DepthAndPlayerIndex : ImageType.Depth);

            //kinect.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(kinect_DepthFrameReady);
        }

        int depthMax = int.MinValue, depthMin = int.MaxValue;

        void kinect_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            DepthImageFrame depthImage = e.OpenDepthImageFrame();

            short[] depthFrame = new short[depthImage.PixelDataLength];

            depthImage.CopyPixelDataTo(depthFrame);
            
            for (int y = 0; y < smallHeight; y++)
            {
                for (int x = 0; x < smallWidth; x++)
                {
                    int originalIndex = GetPixelIndex(origWidth, x * 2, y * 2, 1);
                    int newIndex = GetPixelIndex(smallWidth, smallWidth - x - 1, y, 1);

                    pixel = depthFrame[originalIndex];
                    
                    depthHalfSize[newIndex] = (byte)(~(pixel >> 7));
                }
            }

            
            byte[] compressed = compress(depthHalfSize);

            depthSocket.Send(BitConverter.GetBytes(compressed.Length));

            try
            {
                depthSocket.Send(compressed, compressed.Length, SocketFlags.None);
            }
            catch { }
            

            Console.WriteLine("Bytes on wire: " + (compressed.Length + 4));

            UpdateFps();
        }

        private void InitNetwork()
        {
            if (PrefLoader.GetInt("CheckIP") == 1)
            {
                int firstOctet = PrefLoader.GetInt("FirstIpOctet");

                bool flg = false;
                while (!flg)
                {
                    IPHostEntry entry = Dns.GetHostEntry(Dns.GetHostName());

                    foreach (IPAddress ip in entry.AddressList)
                    {
                        if (ip.GetAddressBytes()[0] == firstOctet)
                        {
                            flg = true;
                            break;
                        }
                    }
                }
            }
            
            

            InitSocket();
        }

        private const int origWidth = 320, smallWidth = 160, smallHeight = 120, origBytesPerPixel = 2;

        private short pixel;
        private byte[] depthHalfSize = new byte[smallWidth * smallHeight * 1];

        private static int GetPixelIndex(int width, int x, int y, int bytesPerPixel)
        {
            return (y * width + x) * bytesPerPixel;
        }

        private void chkFilter_Checked(object sender, RoutedEventArgs e)
        {
            DepthSmoother.UseFiltering = (bool)(sender as CheckBox).IsChecked;
        }

        private void chkNative_Click(object sender, RoutedEventArgs e)
        {
            DepthSmoother.UseNative = (bool)(sender as CheckBox).IsChecked;
        }

        public static byte[] compress(byte[] buffer)
        {
            MemoryStream ms = new MemoryStream();
            GZipStream zip = new GZipStream(ms, CompressionMode.Compress, true);
            zip.Write(buffer, 0, buffer.Length);
            zip.Close();
            ms.Position = 0;

            byte[] compressed = new byte[ms.Length];
            ms.Read(compressed, 0, compressed.Length);

            byte[] gzBuffer = new byte[compressed.Length + 4];
            Buffer.BlockCopy(compressed, 0, gzBuffer, 4, compressed.Length);
            Buffer.BlockCopy(BitConverter.GetBytes(buffer.Length), 0, gzBuffer, 0, 4);
            return gzBuffer;
        }
    }
}

